#include "CvImageGraph.h"


CvImageGraph::CvImageGraph(CvGraph *graph,uint w,uint h)
{
	n_vtxs=0;
	width=w;
	height=h;
	if (graph!=0)
	{
		_graph=graph;
	}
}

CvImageGraph::~CvImageGraph()
{
	Clear();
}
size_t CvImageGraph::NEdges()
{
	return edges.size();
}

uint CvImageGraph::NVts()
{
	return n_vtxs;
}

uint CvImageGraph::VertexIndex(Node n)
{
	return (uint)n;
}
uint CvImageGraph::VertexIndex(uint x,uint y)
{
	return y*width+x+2;
}
uint CvImageGraph::VertexIndex(CvGraphVtx* vtx)
{
	return cvGraphVtxIdx( _graph, vtx ); /*The function cvGraphVtxIdx returns index of the graph vertex.*/
}
uint CvImageGraph::EdgeIndex(uint IndexA,uint IndexB)
{
	return cvGraphEdgeIdx( _graph, Edge(IndexA,IndexB) ); /*The function cvGraphEdgeIdx returns index of the graph edge.*/
}
uint CvImageGraph::EdgeIndex(CvGraphEdge* edge)
{
	return cvGraphEdgeIdx( _graph, edge );
}
uint CvImageGraph::AddVtx()
{
	n_vtxs++;
	return cvGraphAddVtx(_graph,0,0); /*The function cvGraphAddVtx adds a vertex to the graph and returns the vertex index.*/
}
void CvImageGraph::BuildBasicGraph()
{
	AddVtx();
	AddVtx();
	uint i;
	for( i = 0; i < height*width; i++ )/*The function adds vertexes as long as possible.*/
	{
		AddVtx();
	}
}

void CvImageGraph::Clear()
{
	n_vtxs=0;
	edges.clear();
	cvClearGraph(_graph); /*The function cvClearGraph removes all vertices and edges from the graph. The function has O(1) time complexity.*/
}
void CvImageGraph::Connect(uint IndexA,uint IndexB,double weight)
{
	CvGraphEdge* edge;
	cvGraphAddEdge( _graph, IndexA, IndexB, 0, &edge ); /*The function cvGraphAddEdge connects two specified vertices. The function returns 1 if the edge has been added successfully, 0 if the edge connecting the two vertices exists already and -1 if either of the vertices was not found, the starting and the ending vertex are the same or there is some other critical situation. In the latter case (i.e. when the result is negative) the function also reports an error by default.*/
	edge->weight=weight;
	edges.push_back(edge);
}
double CvImageGraph::SumWeights(uint VIndex)
{
	double sum=0,c;
	vector<CvGraphEdge*> edges=NextEdges(VIndex);
	uint i;
	for(i=0;i<edges.size();i++)
		c=EdgeWeight(edges[i]);
		if(c<infinty*0.9)
			sum+=c;
	return sum;
}
CvGraphEdge* CvImageGraph::Edge(uint EdgeIdx)
{
	return edges[EdgeIdx];
}
CvGraphEdge* CvImageGraph::Edge(uint IndexA,uint IndexB)
{
	return cvFindGraphEdge( _graph, IndexA, IndexB ); /*The function cvFindGraphEdge finds the graph edge connecting two specified vertices and returns pointer to it or NULL if the edge does not exists.*/
}
double CvImageGraph::EdgeWeight(CvGraphEdge *edge)
{
	if (edge!=0)
		return edge->weight;
	return 0;
}
double CvImageGraph::EdgeWeight(uint IndexA,uint IndexB)
{
	CvGraphEdge *edge=Edge(IndexA,IndexB);
	return EdgeWeight(edge);
}
vector<CvGraphEdge*> CvImageGraph::VertexEdges(CvGraphVtx* vtx)
{
	return VertexEdges(VertexIndex(vtx));
}
vector<CvGraphEdge*> CvImageGraph::VertexEdges(uint Index)
{
	uint i;
	vector<CvGraphEdge*> edges;
	CvGraphEdge* c_edge;
	for (i=0;i<n_vtxs;i++)
	{
		if (0!= (c_edge=Edge(Index,i)))
			edges.push_back(c_edge);
	}
	return edges;
}
vector<CvGraphEdge*> CvImageGraph::NextEdges(uint Index)
{
	uint i;
	vector<CvGraphEdge*> edges;
	CvGraphEdge* c_edge;
	for (i=1;i<n_vtxs;i++)
	{
		if (0!= (c_edge=Edge(Index,i)))
			edges.push_back(c_edge);
	}
	return edges;
}
vector<uint> CvImageGraph::NextVts(uint Index)
{
	uint i;
	vector<uint> vts;
	for (i=1;i<n_vtxs;i++)
	{
		if (0!= Edge(Index,i))
			vts.push_back(i);
	}
	return vts;
}
void CvImageGraph::push_excess(vector<double> & excess,vector<double> & flow,uint u,uint v)
{
	flow[EdgeIndex(u,v)]-=excess[u];
	excess[u]=0;
	excess[v]+=excess[u];
}
void CvImageGraph::max_flow_push(vector<double> & excess,vector<double> & flow,uint u,uint v)
{
	double send;
	if (EdgeWeight(u,v)>infinty*0.9)
		send=min(excess[u],SumWeights(v));
	else
		send=min(excess[u],EdgeWeight(u,v)-flow[EdgeIndex(u,v)]);
	flow[EdgeIndex(u,v)]+=send;
	excess[u]-=send;
	excess[v]+=send;
}
void CvImageGraph::max_flow_relabel(vector<uint> & height,vector<double> & flow,uint u)
{
	uint min_height=height[u];
	uint i;
	vector<uint> next =NextVts(u);
	for(i=0;i<next.size();i++)
	{
		if (EdgeWeight(u,next[i])-flow[EdgeIndex(u,next[i])]>0)
		{
			min_height=min(min_height, height[next[i]]);
			height[u] = min_height + 1;
		}
	}
}
template <class T>
T maximum(vector<T> v,uint a,uint b)
{
	T max=v[a];
	uint i;
	for(i=a+1;i<b;i++)
	{
		if (max<v[i])
			max=v[i];
	}
	return max;
}
template <class T>
uint find(vector<T> v,T e)
{
	uint i;
	for(i=0;i<v.size();i++)
	{
		if(v[i]==e)
			break;
	}
	return i;
}
void CvImageGraph::max_flow_discharge(vector<uint> & height,vector<double> & excess,vector<uint> & seen,vector<double> & flow,uint u)
{
	uint v;
	vector<uint> next= NextVts(u);
	uint old_height=height[u];
	while (excess[u] > 0)
	{

		if (seen[u]<next.size())
		{
			v = next[seen[u]];
			if ((EdgeWeight(u,v)-flow[EdgeIndex(u,v)]>0) && (height[u] > height[v]))
				max_flow_push(excess,flow,u, v);
			else
				seen[u] += 1;
		}
		else
		{
			max_flow_relabel(height,flow,u);
			seen[u] = 0;
			if (old_height==height[u])
			{
				push_excess(excess,flow,u,source);
			}
			old_height=height[u];
		}

	}

}

vector<double> CvImageGraph::max_flow()
{
	vector<double> flow(NEdges(),0);
	vector<uint> height(NVts(),0);
	vector<double> excess(NVts(),0);
	vector<uint> seen(NVts(),0);
	vector<uint> list;
	uint i;
	for(i=2;i<NVts();i++)
		list.push_back(i);
	height[VertexIndex(source)]=NVts();
	vector<uint> next =NextVts(VertexIndex(source));
	for(i=0;i<next.size();i++)
	{
		excess[VertexIndex(source)]=infinty;
		max_flow_push(excess,flow,VertexIndex(source),next[i]);
	}
	uint p=0,u,old_height;
	uint count=0;
	while (p<NVts()-2)
	{
		u = list[p];
		old_height = height[u];
		max_flow_discharge(height,excess,seen,flow,u);
		if (height[u] > old_height)
		{
            list.insert(list.begin(),list[p]);
			list.erase(list.begin()+p);
            p = 0;
		}
        p += 1;
		if (p>count)
		{
			count=p;
			cout<<100.0*p*p/(NVts()*NVts())<<"%"<<endl;
		}
	}


	return flow;
}
uint CvImageGraph::IndexPx(uint index)
{
	index-=2;
	return index%width;
}
uint CvImageGraph::IndexPy(uint index)
{
	index-=2;
	return index/width;
}
void CvImageGraph::GenerateLabelling(CVImage *labelling,vector<double> flow,int label)
{
	int *intensity;
	uint i;
	for(i=0;i<height*width*2;i++)//see the order of the edge creation
	{
		if (flow[i]==EdgeWeight(edges[i]))
		if (i%2==0)
		{
			intensity=(int*)labelling->Pix((i/2)/height,(i/2)%height,0);
			*intensity=label;
		}
	}
	cout<<"100%"<<endl;
}
